package org.hs.phd.odi.tracking.tracker.particlefilter.resampling;

import java.util.Arrays;
import java.util.Comparator;

import org.hs.phd.odi.tracking.tracker.particlefilter.core.Particle;
import org.hs.phd.odi.tracking.tracker.particlefilter.core.ResamplingStrategy;

public class ImportanceResamplingStrategy<T> implements ResamplingStrategy<T> {

	static class ParticleWeightComparator<T> implements Comparator<Particle<T>> {

		@Override
		public int compare(Particle<T> o1, Particle<T> o2) {
			return (int) Math.signum(o1.getWeight() - o2.getWeight());
		}

	}


	private ParticleWeightComparator<T> comparator;
	
	public ImportanceResamplingStrategy(){
		this.comparator = new ParticleWeightComparator<T>();
	}
	
	
	@SuppressWarnings("unchecked")
	@Override
	public Particle<T>[] resample(Particle<T>[] particles) {


		int particleCount = particles.length;

		Arrays.sort(particles, comparator );

		normalizeWeights(particles);

		Particle<T>[] new_particles = new Particle[particleCount];

		//we loop in reverse, because we want larger weights first
		int k = 0;
outerloop: for (int i = particles.length - 1; i >= 0; i--) {
			int numberOfParticlesCorrespondingToWeight = Math.round(particles[i].getWeight() * particleCount);
			for (int j = 0; j < numberOfParticlesCorrespondingToWeight; j++) {
				new_particles[k++] = particles[i].clone();
				if (k == particleCount)
					break outerloop;
			}
		}
		
		
		while (k < particleCount)
			new_particles[k++] = particles[particles.length - 1].clone();

		return new_particles;
	}
	
	/**
	   Normalizes particle weights so they sum to 1

	   @param particles an array of particles whose weights are to be normalized
	   @param n the number of particles in \a particles
	*/
	private void normalizeWeights(Particle<T>[] particles) {
		float sum = 0;
		int i;

		for (i = 0; i < particles.length; i++)
			sum += particles[i].getWeight();

		for (i = 0; i < particles.length; i++)
			particles[i].setWeight( particles[i].getWeight() / sum );
	}




}
